;PicOne Starter Pack.bas.txt
'StarterStarter Pack FOR WHAM 2 
'=========================================================
' 12/1/2020
;not updated for Pic One yet



#rem
LEDs
====
High 4   ; Right LED on
Low  4   ; Right LED off

High 6   ; centre LED on
Low  6   ; centre LED off

High 6   ; Left LED on
Low  6   ; Left LED off

Wall sensors
============
Readadc   0,b0   ; left wall sensor
Readadc   1,b1   ; front wall sensor
Readadc   2,b2   ; right sensor
*sensor gets number between 0 and 255 depending on its distance from object sensed.

Line sensor
===========
Readadc   3,b3   ; line (...) sensor

Reverse right motor
==============
high 1     ; reverses right motor relay. 
It can only reverse the right motor meaning you can only spin right. 
If you do high 1 then the right motor will go backwards at the speed you programmed. 
If you do low 1 then it puts the right wheel back to going forward.

Read wall Sensors:
==================
high portc 5    ; turns on Infra red illumination
readadc 0,b0 
readadc 1,b1 
readadc 2,b2 
low portc 5  ; turns off Infra red illumination

Motors:
=======
low  portc 1	;right motor off
high portc 2 	;left motor on

pwmout 1,255,1023 ; right_motor full speed
pwmout 1,255,512  ; right_motor half speed
pwmout 1,255,0    ; right_motor stop

pwmout 2,255,1023 ; left full speed
pwmout 2,255,512  ; left half speed
pwmout 2,255,0    ; left stop


pwmout 2,255,left_motor



Push buttons:
=============
if pin6=0 then    ; if start button pressed
do while pin6 =1  : loop  ; wait for start button

Wheel Sensors:
do while pin0 =1  : loop    ; wait for left wheel counter 
  
do while pin3 =1  : loop    ; wait for right wheel counter 
 


; TV remote

irin c.7,b0   ; wait for TV remote key


#endrem


'
'DIRECTIVES
#picaxe 28X1
#no_table        'No data download
'
'SYMBOLS FOR INPUTS/OUTPUTS 
symbol startbutton = pin6  'start button
symbol rmotor = 1
symbol lmotor = 2
symbol LED1 = 7   ; left LED
symbol LED2 = 6   ; centre LED
symbol LED3 = 4   ; right LED

; symbols must use Channel numbers for readadc
symbol Fsensor = 3  ; A.3 

'SYMBOLS FOR Useful numbers
symbol stopspeed = 0   ; PWM output to stop the motors
symbol centre = 128    ; Line sensor centre reading 
symbol Kp =10          ; amount of Derivative. 

'DECLARE SYMBOLS FOR VARIABLES
symbol leftspeed  = W1
symbol rightspeed = W2
symbol value      = W3
symbol oldvalue   = W4
symbol correction = W5
symbol corrected  = W6		; Sensor reading after Derivative applied


symbol leftish = 80
symbol rightish = 160


'************************************************************************
'                        INITIALISATION
'************************************************************************
    gosub setup



; loop to await front button, displaying sensor
do
   readadc Fsensor,value   ; line sensor value
   
   if value > centre then
   	high LED2    ; middle
   else
   	low LED2
   endif
   debug			; debug takes about 500 ms!
loop  while startbutton = 1    ; loop while button not pressed



; loop controlling motor speeds from sensor
top:

do
   oldvalue = value
   readadc Fsensor,value        ; line sensor
   correction = value - oldvalue * Kp    ; extra to add because of rate of change
   corrected = value + correction

   ; check for underflow 
   if corrected > 32000 then
      corrected = 0
   endif
; *******************Your program****************************
; use "corrected" as your sensor reading,
; use "centre" as the line sensor reading for straight
; use "rightspeed" and "leftspeed" as your motorspeeds

   if corrected > rightish then
       high LED3
       low LED1,LED2
       rightspeed = stopspeed +500
       leftspeed  = stopspeed 
   else 
       if  corrected > leftish then
           high LED2
           low LED1,LED3
           rightspeed = stopspeed +500
           leftspeed  = stopspeed +500
       else
           high LED1
           low LED2,LED3
           rightspeed = stopspeed 
           leftspeed  = stopspeed +500 
        endif
    endif

	; this just stops the motors...
  ; rightspeed = stopspeed 
  ; leftspeed  = stopspeed 
; ***********************************************
   gosub checkspeeds  ; check speed values
   gosub setmotors       ; output speeds to motors

loop


;====================SUBROUTINES===============
checkspeeds:
if leftspeed > 32767 then 		; check for underflow
  leftspeed = 0
endif
leftspeed = leftspeed max 1023  ; limit max positive number

if rightspeed > 32767 then 		; check for underflow
  rightspeed = 0
endif
rightspeed = rightspeed max 1023  ; limit max positive number
return

setmotors:
Pwmout 1 ,255,rightspeed 
Pwmout 2 ,255,leftspeed

return

setup:
    setfreq m8                'run faster
   
    ' stop motors
    Pwmout 1 ,255,0 
    Pwmout 2 ,255,0

    sertxd("PicOne Starter Pack.bas.txt",13,10)  ' send message to Terminal window
return



; end of program